import os
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
import cv2
import numpy as np
import glob
import time
from concurrent.futures import ProcessPoolExecutor, ThreadPoolExecutor

class camera:
    def __init__(self,IMAX,DIST):
        self.imtx=np.array(IMAX)#内参矩阵
        self.dist=np.array(DIST)#畸变参数
        self.newcameramtx, self.roi=cv2.getOptimalNewCameraMatrix(self.imtx,self.dist,(1920,1080),0,(1920,1080))
        self.fx=self.imtx[0,0]
        self.fy=self.imtx[1,1]
        self.cx=self.imtx[0,2]
        self.cy=self.imtx[1,2]
        self.f=(self.fx+self.fy)/2
        
    def undistort(self,img):#去畸变 不过这个相机畸变量挺小的 标定出来的畸变参数似乎是假的
        print(self.imtx)
        h,w = img.shape[:2]
        newcameramtx, roi=cv2.getOptimalNewCameraMatrix(self.imtx,self.dist,(w,h),0,(w,h))
        dst = cv2.undistort(img, self.imtx, self.dist, None, self.newcameramtx)
        x,y,w,h = self.roi
        dst = dst[y:y+h, x:x+w]
        print("ok")
        return dst
R=camera(IMAX= [[1.57534870e+03,0.00000000e+00,9.12358094e+02]
 ,[0.00000000e+00,1.57159939e+03,5.11227097e+02]
 ,[0.00000000e+00,0.00000000e+00,1.00000000e+00]],
         DIST=[-0.0651569   ,0.36046137 , 0.00704255 ,-0.00412844 ,-0.88801585])
print("ok0")
cap = cv2.VideoCapture(0,cv2.CAP_MSMF)#打开内置摄像机
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,1080)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,3840)
while cap.isOpened():#当摄像头打开时
    print("ok1")
    ret,frame=cap.read()#读取当前摄像头画面
    l=frame[:,0:1920]
    r=frame[:,1920:3840]
    r=R.undistort(r)
    cv2.imshow('r',r)
    cv2.waitKey(1)
